//-----------------------Bibliotecas-----------------------------------------
#include "../ManipulacaoMatrizes/Matrizes.h"
#include "ManipulacaoFrames.h"
#include <math.h>
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
//----------------Algoritmos de Manipulacao de Transformacoes----------------
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
float **_RotX(float Angulo)
{
   float **Matriz;
   float   Seno;
   float   Coseno;


   Matriz = CriaMatriz(4,4);
   Angulo = (Angulo*M_PI)/180;

   for(int i=0;i<4;i++)
      for(int j=0;j<4;j++)
         Matriz[i][j] = 0;

   Seno   = sin(Angulo);
   Coseno = cos(Angulo);

   Matriz[0][0] =  1;
   Matriz[3][3] =  1;
   Matriz[1][1] =  Coseno;
   Matriz[2][2] =  Coseno;
   Matriz[1][2] = -Seno;
   Matriz[2][1] =  Seno;

   return(Matriz);
}
//---------------------------------------------------------------------------
float **_RotY(float Angulo)
{
   float **Matriz;
   float   Seno;
   float   Coseno;


   Matriz = CriaMatriz(4,4);
   Angulo = (Angulo*M_PI)/180;

   for(int i=0;i<4;i++)
      for(int j=0;j<4;j++)
         Matriz[i][j] = 0;

   Seno   = sin(Angulo);
   Coseno = cos(Angulo);

   Matriz[0][0] =  Coseno;
   Matriz[3][3] =  1;
   Matriz[1][1] =  1;
   Matriz[2][2] =  Coseno;
   Matriz[0][2] =  Seno;
   Matriz[2][0] = -Seno;

   return(Matriz);
}
//---------------------------------------------------------------------------
float **_RotZ(float Angulo)
{
   float **Matriz;
   float   Seno;
   float   Coseno;


   Matriz = CriaMatriz(4,4);
   Angulo = (Angulo*M_PI)/180;

   for(int i=0;i<4;i++)
      for(int j=0;j<4;j++)
         Matriz[i][j] = 0;

   Seno   = sin(Angulo);
   Coseno = cos(Angulo);

   Matriz[2][2] =  1;
   Matriz[3][3] =  1;
   Matriz[0][0] =  Coseno;
   Matriz[1][1] =  Coseno;
   Matriz[1][0] =  Seno;
   Matriz[0][1] = -Seno;

   return(Matriz);
}
//---------------------------------------------------------------------------
float **_Trans(float X,float Y,float Z)
{
   float **Matriz;

   Matriz = CriaMatriz(4,4);

   for(int i=0;i<4;i++)
      for(int j=0;j<4;j++)
         Matriz[i][j] = 0;

   Matriz[0][0] = 1;
   Matriz[1][1] = 1;
   Matriz[2][2] = 1;
   Matriz[3][3] = 1;
   Matriz[0][3] = X;
   Matriz[1][3] = Y;
   Matriz[2][3] = Z;

   return(Matriz);
}
//---------------------------------------------------------------------------
float *ExtraiParametros(float **MatrizTransformacao)
{
   ///////////////////////////////////////////////////////////
   //               Retorna um Vetor com                    //
   //         X | AnguloX | Y | AnguloY | Z | AnguloZ       //
   ///////////////////////////////////////////////////////////

   float *Resultado;
   float  Apoio;

   Resultado = CriaVetor(6);

   Resultado[0] = MatrizTransformacao[0][3];
   Resultado[2] = MatrizTransformacao[1][3];
   Resultado[4] = MatrizTransformacao[2][3];

   Resultado[1] = M_1_PI*180*atan2(MatrizTransformacao[2][1],MatrizTransformacao[2][2]);

   Resultado[5] = atan2(MatrizTransformacao[1][0],MatrizTransformacao[0][0]);

   Apoio        = MatrizTransformacao[0][0]*cos(Resultado[5]) + MatrizTransformacao[1][0]*sin(Resultado[5]);
   Resultado[3] = M_1_PI*180*atan2(-MatrizTransformacao[2][0],Apoio);

   Resultado[5] = M_1_PI*180*Resultado[5];

   return(Resultado);
}
//---------------------------------------------------------------------------
